﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using SpriteSheetRuntime;
using Microsoft.Kinect.Toolkit.Interaction;


namespace FormaBedoboNS
{
    /// <summary>
    /// A szenzor kezelése ebben az osztályban történik
    /// </summary>
    public class KinectManager
    {
        FormaBedoboGame mGame;
        KinectSensor mKinect;
        Color[] mLatestColorData;


        Skeleton[] skeletonData;

        /// <summary>
        /// Contvázak tárolója
        /// </summary>
        public List<Skeleton> Skeletons;

        Dictionary<int, UserInfo> UserInfos;

        bool gotofullscren = true;
        bool stop = false;

        /// <summary>
        /// a 0. csontváz jobb kezének pozíciója, ha nem használunk Kinect.Toolkit.Interaction-t
        /// </summary>
        public Vector3 rightHandPosition{get;set;} 
        /// <summary>
        /// a 0. csontváz bal kezének pozíciója, ha nem használunk Kinect.Toolkit.Interaction-t
        /// </summary>
        public Vector3 leftHandPosition{get;set;}
        /// <summary>
        /// a 0. csontváz jobb kezének pozíciója
        /// </summary>
        public Vector2 rightHandPosition2 { get; set; }
        /// <summary>
        /// bal kéz
        /// </summary>
        public InteractionHandPointer lefthandPointer;
        /// <summary>
        /// jobb kéz
        /// </summary>
        public InteractionHandPointer righthandPointer;

        InteractionStream interactionStream;

        /// <summary>
        /// a bal tenyér markol-e?
        /// </summary>
        public bool IsLeftGrabbing=false;
        /// <summary>
        /// a bal tenyér markol-e?
        /// </summary>
        public bool IsRightGrabbing = false;

        /// <summary>
        /// a bal tenyér elengedett-e?
        /// </summary>
        public bool IsLeftRelease = false;
        
        /// <summary>
        /// melyik csontvázat követjük épp
        /// </summary>
        public int Skeleton_TrackingId=-1;

        /// <summary>
        /// ctor
        /// </summary>
        /// <param name="g"></param>
        public KinectManager(FormaBedoboGame g)
        {
            mGame = g;
             
        }
        /// <summary>
        /// kinect unload
        /// </summary>
        public void stopKinect()
        {
            stop = true;
        }

        /// <summary>
        /// fut-e a Kinect?
        /// </summary>
        /// <returns></returns>
        public bool runningKinect()
        {
            bool ret = false;
            if (mKinect != null)
                ret = true;
            return ret;
        }
        /// <summary>
        /// Kinect inicializálás
        /// </summary>
        /// <returns></returns>
        public string initKinect()
        {
            string ret = "" ;
            if (KinectSensor.KinectSensors.Count == 0)
            {
                return "Error";
            }
            mKinect = KinectSensor.KinectSensors[0];

            interactionStream = new InteractionStream(mKinect, new InteractionClient());
            Skeletons = new List<Skeleton>();
            UserInfos = new Dictionary<int, UserInfo>();
            try
            {
                mKinect.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
                mKinect.ColorFrameReady += new EventHandler<ColorImageFrameReadyEventArgs>(mKinect_ColorFrameReady);
                mKinect.SkeletonStream.Enable(); // Enable skeletal tracking
                mKinect.DepthStream.Enable();
            }
            catch(Exception e)
            {
                Console.WriteLine(e.Message);
                return "Error";
            }

            //mKinect.SkeletonStream.TrackingMode = SkeletonTrackingMode.Seated; 

            skeletonData = new Skeleton[mKinect.SkeletonStream.FrameSkeletonArrayLength]; // Allocate ST data

            //mKinect.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(kinect_SkeletonFrameReady); // Get Ready for Skeleton Ready Events

            mKinect.Start();
            
            try
            {
                if (AppSettings.instance.GetKinectElevation() != mKinect.ElevationAngle)
                {
                    Console.WriteLine("fej kezdő pozíció beállítás");
                    mKinect.ElevationAngle = AppSettings.instance.GetKinectElevation();
                    Console.WriteLine("fej kezdő pozíció beállítás vége");
                }
            }
            catch (Exception)
            {
                Console.WriteLine("fej kezdő pozíció állítás hiba");
            }
            
            return ret;
        }

        /// <summary>
        /// Kinect fej billentési szög változtatás
        /// </summary>
        /// <param name="i">ennyi fokkal változik a fej szöge</param>
        /// <returns>a beállítás sikerességét adja vissza</returns>
        public bool fejAllitas(int i)
        {
            bool ret = true;
            try
            {
                mKinect.ElevationAngle += i;
                AppSettings.instance.KinectElevation(mKinect.ElevationAngle);
            }
            catch (Exception)
            {
                ret = false;
            }
            return ret;
        }

        /// <summary>
        /// Mélységi kamera adatainak feldolgozása
        /// </summary>
        void ProcessDepthFrame()
        {
            using (var dif = this.mKinect.DepthStream.OpenNextFrame(0))
            {
                if (dif != null)
                {
                    DepthImagePixel[] data = new DepthImagePixel[dif.PixelDataLength];
                    dif.CopyDepthImagePixelDataTo(data);

                    //Feed depth data to interactionStream
                    interactionStream.ProcessDepth(data, dif.Timestamp);
                }
            }
        }

        /// <summary>
        /// ProcessSkeletonFrame
        /// </summary>
        void ProcessSkeletonFrame()
        {
            using (var skeletonFrame = this.mKinect.SkeletonStream.OpenNextFrame(0))
            {
                if (skeletonFrame != null)
                {
                    Skeleton[] skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
                    skeletonFrame.CopySkeletonDataTo(skeletons);

                    //Feed skeleton data to interactionStream
                    var accelerometerReading = this.mKinect.AccelerometerGetCurrentReading();
                    interactionStream.ProcessSkeleton(skeletons, accelerometerReading, skeletonFrame.Timestamp);

                    Skeletons.Clear();
                    foreach (Skeleton skel in skeletons)
                    {
                        if (skel.TrackingState == SkeletonTrackingState.Tracked)
                        {
                            Skeletons.Add(skel);
                        }
                    }
                }
            }
        }
        /// <summary>
        /// ProcessInteractionFrame
        /// </summary>
        void ProcessInteractionFrame()
        {
            using (var interactionFrame = interactionStream.OpenNextFrame(0))
            {
                if (interactionFrame != null)
                {
                    UserInfo[] userInfo = new UserInfo[InteractionFrame.UserInfoArrayLength];
                    interactionFrame.CopyInteractionDataTo(userInfo);

                    //Query the UserInfo for every skeleton and save it in a dictionary
                    UserInfos.Clear();
                    foreach (var skel in Skeletons)
                    {
                        var ui = (from UserInfo u in userInfo where u.SkeletonTrackingId == skel.TrackingId select u).FirstOrDefault();
                        if (ui != null)
                        {
                            Skeleton_TrackingId = skel.TrackingId;
                            UserInfos.Add(skel.TrackingId, ui);
                        }
                    }
                }
            }
        }
        /// <summary>
        /// Csontváz feldolgozásához
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        void kinect_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            if (stop)
            {
                mKinect.Stop();
            }
            
            using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
            {
                
                if (skeletonFrame != null)
                {
                    //int skeletonSlot = 0;
                    if (gotofullscren && rightHandPosition.Z>0)
                    {
                        gotofullscren = false;
                        mGame.FullScreen();
                    }
                    Skeleton[] skeletonData = new Skeleton[skeletonFrame.SkeletonArrayLength];

                    skeletonFrame.CopySkeletonDataTo(skeletonData);
                    Skeleton playerSkeleton = (from s in skeletonData where s.TrackingState == SkeletonTrackingState.Tracked select s).FirstOrDefault();
                    if (playerSkeleton != null)
                    {
                        Joint rightHand = playerSkeleton.Joints[JointType.HandRight];
                        Joint leftHand = playerSkeleton.Joints[JointType.HandLeft];
                        
                        rightHandPosition = new Vector3((((0.8f * rightHand.Position.X) + 0.5f) * (FormaBedoboGame.screenManager.GraphicsDevice.Viewport.Width)), (((-0.8f * rightHand.Position.Y) + 0.5f) * (FormaBedoboGame.screenManager.GraphicsDevice.Viewport.Height)), rightHand.Position.Z);
                        leftHandPosition = new Vector3((((0.8f * leftHand.Position.X) + 0.5f) * (FormaBedoboGame.screenManager.GraphicsDevice.Viewport.Width)), (((-0.8f * leftHand.Position.Y) + 0.5f) * (FormaBedoboGame.screenManager.GraphicsDevice.Viewport.Height)), rightHand.Position.Z);


                    }
                }
            }
        }
        /// <summary>
        /// Kamerakép feldolgozásához
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        void mKinect_ColorFrameReady(object sender, ColorImageFrameReadyEventArgs e)
        {
            if (!mGame.drawColorStream)
                return;
            ColorImageFrame frame = e.OpenColorImageFrame();
            if (frame == null)
            {
                return;
            }
            byte[] pixelData = new byte[frame.PixelDataLength];
            frame.CopyPixelDataTo(pixelData);

            mLatestColorData = new Color[pixelData.Length / 4];

            int offset = 0;

            for (int i = 0; i < mLatestColorData.Length; i++)
            {
                mLatestColorData[i] = new Color(pixelData[offset + 2], pixelData[offset + 1], pixelData[offset]);
                offset += 4;
            }
            frame.Dispose();


        }
        /// <summary>
        /// a kamerakép elkérése
        /// </summary>
        /// <returns></returns>
        public Color[] getLatestColorData()
        {

            if (mLatestColorData != null)
            {
                return mLatestColorData;
            }
            return null;
        }
        
        /// <summary>
        /// a kéz pozíciójának elkérése
        /// </summary>
        /// <param name="ht"></param>
        /// <returns></returns>
        private InteractionHandPointer getHandPointer(Microsoft.Kinect.Toolkit.Interaction.InteractionHandType ht)
        {
            UserInfo userInfo;
            //var hand = JointType.HandLeft;
            var hand = ht;

            //if (UserInfos.TryGetValue(Player.Skeleton.TrackingId, out userInfo))
            if (UserInfos.TryGetValue(Skeleton_TrackingId, out userInfo))
            {
                return (from InteractionHandPointer hp in userInfo.HandPointers where hp.HandType == hand select hp).FirstOrDefault();
            }

            return null;
        }

        /// <summary>
        /// update
        /// </summary>
        public void Update()
        {
            if (this.mKinect != null)
            {
                //ProcessColorFrame();
                ProcessSkeletonFrame();
                ProcessDepthFrame();
                ProcessInteractionFrame();

                lefthandPointer = getHandPointer(Microsoft.Kinect.Toolkit.Interaction.InteractionHandType.Left);
                righthandPointer = getHandPointer(Microsoft.Kinect.Toolkit.Interaction.InteractionHandType.Right);
                
                if (lefthandPointer != null)
                {
                    if ((lefthandPointer.HandEventType == InteractionHandEventType.GripRelease) && IsLeftGrabbing)
                    {
                        IsLeftRelease = true;
                    }
                    else
                        IsLeftRelease = false;

                    if (lefthandPointer.HandEventType == InteractionHandEventType.Grip)
                    {
                        IsLeftGrabbing = true;
                        Console.WriteLine("LeftGrabbing");
                    }
                    else if (lefthandPointer.HandEventType == InteractionHandEventType.GripRelease)
                    {
                        IsLeftGrabbing = false;
                    }
                    
                }

                if (righthandPointer != null)
                {
                    rightHandPosition2 = new Vector2((float)(((0.5f * righthandPointer.X) + 0.5f) * (FormaBedoboGame.screenManager.GraphicsDevice.Viewport.Width) - FormaBedoboGame.screenManager.GraphicsDevice.Viewport.Width / 2), (float)(((0.5f * righthandPointer.Y) + 0.5f) * (FormaBedoboGame.screenManager.GraphicsDevice.Viewport.Height) - FormaBedoboGame.screenManager.GraphicsDevice.Viewport.Height/2));
                    
                    if (righthandPointer.HandEventType == InteractionHandEventType.Grip)
                    {
                        IsRightGrabbing = true;
                        Console.WriteLine("RightGrabbing");
                    }
                    else if (righthandPointer.HandEventType == InteractionHandEventType.GripRelease)
                    {
                        IsRightGrabbing = false;
                    }
                }
            }
        }

    }

    /// <summary>
    /// Kinect.Toolkit.Interaction InteractionClient
    /// </summary>
    class InteractionClient : IInteractionClient
    {
        public InteractionInfo GetInteractionInfoAtLocation(int skeletonTrackingId, InteractionHandType handType, double x, double y)
        {
            var info = new InteractionInfo();
            info.IsGripTarget = true;
            info.IsPressTarget = false;
            info.PressAttractionPointX = 0f;
            info.PressAttractionPointY = 0f;
            info.PressTargetControlId = 0;

            return info;
        }
    }
}
